package cobots.swarm.main;

import java.awt.geom.Point2D;
import java.util.Collection;
import java.util.Collections;
import java.util.Comparator;
import java.util.LinkedHashMap;
import java.util.LinkedList;
import java.util.Vector;

import cobots.swarm.agents.Agent;
import cobots.swarm.agents.FollowerAgent;
import cobots.swarm.agents.LeaderAgent;
import cobots.swarm.agents.Target;
import cobots.swarm.props.PropertiesClass;
import cobots.swarm.view.MainViewClass;


public class MainSwarmClass extends Thread {

    protected double timeStep = PropertiesClass.getSimStep();
    protected boolean started = false;
    protected double targetProbability = PropertiesClass.getTprob();
    protected double maxTime = PropertiesClass.getmaxTime();
    protected double commProbability = PropertiesClass.getCprob();
    protected double operatorCommProb = PropertiesClass.getOprob();

    protected int teamMembers = PropertiesClass.getteamMembers();
    protected boolean userSelectsTeam = PropertiesClass.getfixedRadius();
    public static final double g = PropertiesClass.getg();
    public static final double Fmax = PropertiesClass.getFmax();
    public static final double criticalDist = 100;
    protected static final double iRadius = PropertiesClass.getRmean() * 1.5;
    protected LinkedHashMap<Integer, Float> targetAges = new LinkedHashMap<Integer, Float>();
    protected LinkedHashMap<Integer, Vector<Integer>> topology = new LinkedHashMap<Integer, Vector<Integer>>();

    public MainSwarmClass(int fs, int leaders) {

            SwarmState.init(leaders, fs);
      
        MainViewClass.main(new String[0]);
        
        start();
    }
    
    @Override
    public void run() {
        while (maxTime == 0 || SwarmState.currentTime < maxTime) {
            computeStep();
            SwarmState.updateTargetAges();
            try {
                Thread.yield();
            } catch (Exception e) {
                e.printStackTrace();
            }
        }
        System.out.println("Reached maximum time of " + maxTime);
        System.exit(0);
    }
    
    public Collection<Short> computeNeighbors(short id) {
        LinkedList<Short> neighbors = new LinkedList<Short>();
        Agent pivot = SwarmState.getRobot(id);
        
        for (Agent p : SwarmState.getAllRobots()) {
            if (p.distance(pivot) <= iRadius) {
                short nid = p.getId();
                neighbors.add(nid);
            }
        }

        return neighbors;
    }

    public Collection<Short> computeFollowers(short leaderId) {
        final LeaderAgent leader = SwarmState.leader(leaderId);
        LinkedList<FollowerAgent> freeFollowers = SwarmState.getFreeFollowers();

        // nearest-n
        if (!userSelectsTeam) {
            Collections.sort(freeFollowers, new Comparator<FollowerAgent>() {

                public int compare(FollowerAgent o1, FollowerAgent o2) {
                    if (o1.distance(leader) <= o2.distance(leader))
                        return -1;
                    return 1;
                }
            });
            Vector<Short> followers = new Vector<Short>();
            for (int i = 0; i < teamMembers && !freeFollowers.isEmpty(); i++) {
                FollowerAgent f = freeFollowers.removeFirst();
                if (Math.random() > (1.0 - commProbability)) {
                    followers.add(f.getId());
                }
            }
            return followers;
        }



        return new Vector<Short>();
    }

    public void computeStep() {

        //TODO compute step correctly
        SwarmState.currentTime += timeStep;

        if (Math.random() > (1 - targetProbability)) {
            Target t = new Target();
            SwarmState.targets.put(t.getId(), t);
        }

        LinkedHashMap<Short, Point2D> velocities = new LinkedHashMap<Short, Point2D>();
        for (Short f : SwarmState.getLeaderIds()) {
            LeaderAgent leaderrobot = SwarmState.leader(f);
            if (leaderrobot.getTargetId() != -1 && !leaderrobot.isComputedCoalition()) {
            	Collection<Short> followers = computeFollowers(leaderrobot.getId());
            	for (short s : followers)
            		SwarmState.follower(s).setLeaderId(leaderrobot.getId());
            	leaderrobot.setComputedCoalition(true);
            }
            velocities.put(f, leaderrobot.computeMovement());
        }

        for (Short f : SwarmState.getFollowerIds()) {
            FollowerAgent followerrobot = SwarmState.follower(f);
            velocities.put(f, followerrobot.computeMovement());
        }

        //update velocities
        for (Short s : velocities.keySet()) {
            Agent p = SwarmState.getRobot(s);
            
            Point2D pt = new Point2D.Double(p.getX()+  velocities.get(s).getX(),  p.getY()
                    +  velocities.get(s).getY());
            
            if (pt.getX() < SwarmState.bounds.getMinX())
            	pt.setLocation(SwarmState.bounds.getMinX(), pt.getY());
            else if (pt.getX() > SwarmState.bounds.getMaxX())
            	pt.setLocation(SwarmState.bounds.getMaxX(), pt.getY());            
            
            if (pt.getY() < SwarmState.bounds.getMinY())
            	pt.setLocation(pt.getX(), SwarmState.bounds.getMinY());
            else if (pt.getY() > SwarmState.bounds.getMaxY())
            	pt.setLocation(pt.getX(), SwarmState.bounds.getMaxY());
                        
            p.setLocation(pt);
        }

        Vector<Short> toRemove = new Vector<Short>();

        for (Target t : SwarmState.targets.values()) {
            SwarmState.points += timeStep / 1000.0;
            for (Agent p : SwarmState.getAllRobots()) {
                if (p.distance(t) < 2) {
                    t.setMass(t.getMass() - 0.00001 * p.getAbsorption());   // At this point the target is decreasing 
                }
            }
            if (t.getMass() <= 0)
                toRemove.add(t.getId());
        }

        for (Short s : SwarmState.getLeaderIds()) {
            LeaderAgent r = SwarmState.leader(s);
            if (toRemove.contains(r.getTargetId())) {
                r.setTargetId(-1);
                for (FollowerAgent f : SwarmState.getFollowers(r.getId())) {
                    f.setLeaderId(-1);
                }
            }            
        }

        for (Short key : toRemove) {
        
            SwarmState.targets.remove(key);
        }
        
        MainViewClass.updatePartialVisualization();
    }

    public Agent r(int id) {
        return SwarmState.getRobot((short) id);
    }

    public Target t(int id) {
        return SwarmState.target(id);
    }

    public LinkedHashMap<Integer, Float> getTargetAges() {
		return targetAges;
	}

	public static void main(String[] args) {
         new MainSwarmClass(PropertiesClass.getnumFollowers(), PropertiesClass.getnumLeaders());
    }
}
